function res = PNGguidanceloop(resp)
global hInt N1 N2 
t = resp(1) + hInt;
statep = resp(2:7);
x = statep(1);
y = statep(2);
z = statep(3);
V = statep(4);
fpa = statep(5);
azi = statep(6);

SMT = sqrt(x^2+y^2);
LMT = sqrt(x^2+y^2+z^2);
lambda = atan(z/SMT);
if x >= 0 
    phi = asin(y/SMT);
else
    phi = -(asin(y/SMT)+pi);
end

% look angle
delta = fpa + lambda;
eta = azi + phi + pi/2;

lambdadot = V*(sin(fpa)*cos(lambda)+cos(fpa)*sin(lambda)*cos(azi+phi+pi/2))/LMT;
phidot = V*cos(fpa)*cos(azi+phi)/LMT/cos(lambda);
fpadotd = -N1*lambdadot;
azidotd = -N2*phidot;
[alphad, bankd] = calCommands(z, V, fpadotd, azidotd, fpa);
state = rk4(@TerminalDynamic, statep, alphad, bankd, hInt);
[r, dr] = calRandDR(state);

res(1) = t;
res(2:7) = state;
res(8) = lambda;
res(9) = phi;
res(11) = alphad;
res(12) = bankd;
res(14) = r;
res(15) = dr;
res(16) = delta;
res(17) = eta;